#include "Vtl/geometry.h"
#include <Eigen/StdVector>
#include <utility>
#include <map>

namespace Vtl
{
	namespace intersections
	{
		template <typename Scalar, int Dimension>
		inline bool lineWithLineIntersection(const Eigen::ParametrizedLine<Scalar, Dimension> &_LineA,
			const Eigen::ParametrizedLine<Scalar, Dimension> &_LineB, 
			Eigen::Matrix<Scalar, Dimension, 1> *_Point /*= NULL*/,
			const double &_Eps /*= 1e-4*/)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			typedef Eigen::ParametrizedLine<Scalar,Dimension> ParametrizedLine;

			ParametrizedLine seg = lineToLineSegment(_LineA, _LineB, _Eps);			
			double sqr_dist = seg.direction().squaredNorm();
			if(sqr_dist<_Eps)
			{
				if(_Point!=NULL)
					*_Point = seg.origin();
				return (true);
			}
			return (false);
		}

		template <typename Scalar, int Dimension>
		inline Eigen::ParametrizedLine<Scalar, Dimension> lineToLineSegment(const Eigen::ParametrizedLine<Scalar, Dimension> &_LineA,
			const Eigen::ParametrizedLine<Scalar, Dimension> &_LineB,
			const double &_Eps /*= 1e-4*/)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			typedef Eigen::ParametrizedLine<Scalar,Dimension> ParametrizedLine;

			// point + direction = 2nd point
			VectorType p1 = _LineA.origin();
			VectorType dir1 = _LineA.direction();
			VectorType p2 = p1 + dir1;

			// point + direction = 2nd point
			VectorType q1 = _LineB.origin();
			VectorType dir2 = _LineB.direction();
			VectorType q2 = q1 + dir2;

			// a = x2 - x1 = line_a[1] - line_a[0]
			VectorType u = dir1;
			// b = x4 - x3 = line_b[1] - line_b[0]
			VectorType v = dir2;
			// c = x2 - x3 = line_a[1] - line_b[0]
			VectorType w = p2 - q1;

			double a = u.dot (u);
			double b = u.dot (v);
			double c = v.dot (v);
			double d = u.dot (w);
			double e = v.dot (w);
			double denominator = a*c - b*b;
			double sc, tc;
			// Compute the line parameters of the two closest points
			if (denominator < _Eps)          // The lines are almost parallel
			{
				sc = 0.0;
				tc = (b > c ? d / b : e / c);  // Use the largest denominator
			}
			else
			{
				sc = (b*e - c*d) / denominator;
				tc = (a*e - b*d) / denominator;
			}
			// Get the closest points

			VectorType pt1 = p2 + sc * u;
			VectorType pt2 = q1 + tc * v;
			return ParametrizedLine(pt1, (pt2 - pt1));
		}

		template <typename Scalar, int Dimension>
		inline bool lineWithFaceIntersection(const Eigen::ParametrizedLine<Scalar, Dimension> &_Line,
			const Polygon<Scalar, Dimension> &_Face,
			Eigen::Matrix<Scalar, Dimension, 1> *_Point /*= NULL*/,
			const double &_Eps /*= 1e-4*/)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			typedef Eigen::Hyperplane<Scalar, Dimension> Hyperplane;

			if(_Face.size()<3)
				return false;

			VectorType u = _Face[1] - _Face[0];
			VectorType v = _Face[2] - _Face[1];
			VectorType normal = u.cross(v).normalized();

			if(geometry::vectorParallelTovector(u,_Line.direction()))
				return false;

			Hyperplane plane(normal, _Face[0]);
			Scalar t = _Line.intersection(plane);
			VectorType point = _Line.origin() + _Line.direction() * t;

			if(powl(t,2) - _Line.direction().squaredNorm() > _Eps)
				return false;

			if(!geometry::pointInPolygon(point, _Face))
				return false;

			if(_Point != NULL)
				*_Point = point; 

			return true;


		}

		template <typename Scalar, int Dimension>
		inline bool lineWith2DPolygonIntersection(const Eigen::ParametrizedLine<Scalar, Dimension> &_Line,
			const Polygon<Scalar, 2> &_2DPolygon,
			std::vector<Eigen::Matrix<Scalar, 2, 1>, Eigen::aligned_allocator<Eigen::Matrix<Scalar, 2, 1>>> *_Points /*= NULL*/,
			const double &_Eps /*= 1e-4*/)
		{
			typedef Eigen::Matrix<Scalar, 2, 1> VectorType;
			typedef Eigen::ParametrizedLine<Scalar, 2> ParametrizedLine;
			typedef std::map<Scalar, VectorType, std::less<Scalar>, Eigen::aligned_allocator<std::pair<Scalar, VectorType>>> IntersectionMap;
			typedef IntersectionMap::value_type MapValueType;
			typedef IntersectionMap::iterator IntersectionMapIterator;

			ParametrizedLine line = common::convertTo2D(_Line);
			if(!_2DPolygon.getBoundingBox().IntersectWithLine(line))
				return false;

			IntersectionMap intersections;

			for (int a=0,b=1,num_ab = _2DPolygon.getVertexCount();
				a<num_ab;++a,++b)
			{
				ParametrizedLine edge;
				if(b<num_ab)
					edge = ParametrizedLine(_2DPolygon[a],_2DPolygon[b] - _2DPolygon[a]);
				else
					edge = ParametrizedLine(_2DPolygon[a],_2DPolygon[0] - _2DPolygon[a]);

				VectorType point;
				if(lineWithLineIntersection(line, edge, &point, _Eps) && 
					geometry::pointOnLineSegment(point, edge, _Eps))
				{
					if(_Points==NULL)
						return true;
					intersections.insert(MapValueType(distance(line.origin(), point), point));
				}

			}
			if(_Points!= NULL &&
				intersections.size()>0)
			{
				for(IntersectionMapIterator i = intersections.begin(),
					end = intersections.end();
					i!=end;i++)
				{
					_Points->push_back(i->second);
				}
				return true;
			}
			return false;
		}

		template <typename Scalar, int Dimension>
		inline bool lineWithExcludedPolygonIntersection(const Eigen::ParametrizedLine<Scalar, Dimension> &_Line,
			const Polygon<Scalar, 2> &_2DPolygon,
			const Scalar &_PolygonBaseLineHeight,
			const Scalar &_PolygonHeight,
			Eigen::Matrix<Scalar, Dimension, 1> *_Point /*= NULL*/,
			const double &_Eps /*= 1e-4*/)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			typedef Eigen::Matrix<Scalar, 2, 1> VectorType2D;
			typedef Eigen::ParametrizedLine<Scalar, Dimension> ParametrizedLine;
			typedef Eigen::ParametrizedLine<Scalar, 2> ParametrizedLine2D;

			std::vector<VectorType2D, Eigen::aligned_allocator<VectorType2D>> points;
			if(lineWith2DPolygonIntersection(_Line, _2DPolygon, &points, _Eps))
			{
				VectorType origin = common::convertTo3D(points[0], _PolygonBaseLineHeight);
				VectorType direction = common::convertTo3D(points[0], _PolygonBaseLineHeight+_PolygonHeight) - origin;
				ParametrizedLine vertical_line(origin, direction);
				VectorType point;
				if(lineWithLineIntersection(_Line, vertical_line, &point, _Eps) && 
					geometry::pointOnLineSegment(point, vertical_line, _Eps))
				{
					if(_Point != NULL)
						*_Point = point;

					return true;
				}
			}
			return false;
		}
	};
};